import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from logging import DEBUG,INFO,WARNING,ERROR,FATAL
from Manipulator import myManipulator,Servo
import numpy as np


class JoyMoveArm(Node):

    class PosVec:
        def __init__(self,parent:Node,x:float,y:float,z:float):
            self.x:float=x
            self.y:float=y
            self.z:float=z
            self.__parent_instance=parent


        def set_pos(self,pos:tuple):
            pos_len=len(pos)
            if(pos_len!=3):
                self.__parent_instance.get_logger().warn("Pos len should be 3, not {}".format(pos_len))
                raise ValueError("PosVec: Tuple len error")
            self.x=pos[0]
            self.y=pos[1]
            self.z=pos[2]


    class AxesRatio:
        """
        摇杆值缩放倍率
        """
        def __init__(self):
            self.lx_ratio,self.ly_ratio,self.rx_ratio,self.ry_ratio=(1,1,1,1)


    def __init__(self,
                 node_name:str,
                 timer_period_ms:np.uint16,
                 arm_period_ms:np.uint16,
                 ratios:AxesRatio,
                   *, context = None, cli_args = None, namespace = None, use_global_arguments = True, enable_rosout = True, start_parameter_services = True, parameter_overrides = None, allow_undeclared_parameters = False, automatically_declare_parameters_from_overrides = False):
        """
        订阅/joy话题并根据消息控制机械臂末端移动
        """
        super().__init__(node_name, context=context, cli_args=cli_args, namespace=namespace, use_global_arguments=use_global_arguments, enable_rosout=enable_rosout, start_parameter_services=start_parameter_services, parameter_overrides=parameter_overrides, allow_undeclared_parameters=allow_undeclared_parameters, automatically_declare_parameters_from_overrides=automatically_declare_parameters_from_overrides)

        self.__arm_period_ms=arm_period_ms  # 机械臂刷新控制周期
        self.target_pos=self.PosVec(self,0,0,0)  # 机械臂末端目标位置
        self.__arm:myManipulator=None   # 机械臂实例对象
        self.__joy_sub=None # /joy话题订阅者
        self.__ctrl_timer=None   # 控制循环定时器
        self.__last_axe_cmds=np.zeros(6)   # 上次消息命令存储: 摇杆
        self.__last_buttom_cmds=np.zeros(21)    # 上次消息命令存储: 按键
        self.__ratios=ratios    # 摇杆值-步长放大倍率

        self.get_logger().set_level(DEBUG)
        self.get_logger().info("节点已启动: {}".format(node_name))

        # 订阅/joy话题
        self.__joy_sub=self.create_subscription(Joy,"joy",self.__sub_callback,10)

        # 创建定时器, 定时控制机械臂
        self.__ctrl_timer=self.create_timer(float(timer_period_ms*0.001),self.__timer_callback)

        # 初始化舵机 & 机械臂
        servo=Servo("/dev/ttyAMA4",9600)
        self.__arm=myManipulator([(65,130,130),(71,-20-1.12,0)],self.get_logger(),servo)

        # 动作组移动机械臂末端到初始位置, 读取并装载当前位置
        initial_pos=(190,0,5)
        self.target_pos.set_pos(initial_pos)
        self.__arm_pos_ctrl(500)


    def __arm_pos_ctrl(self,time_ms:np.uint16=None)->bool:
        """
        控制机械臂末端以P2P模式运动到目标位置
        """
        if(time_ms==None):
            time_ms=self.__arm_period_ms
        return self.__arm.Goto_Target_Pos(self.target_pos,time_ms)


    def __timer_callback(self):
        """定时器回调, 控制机械臂运动到目标位置"""
        try:
            busy_flag=self.__arm_pos_ctrl()
        except Exception as e:
            self.get_logger().error(e)


    def __sub_callback(self,cmd:Joy):
        """
        收到/joy话题消息回调, 根据按键、摇杆值刷新目标位置
        """
        # 摇杆值
        axe_cmds=cmd.axes
        # 按键值
        buttom_cmds=cmd.buttons

        lx,ly,rx,ry=axe_cmds[0:4]   # 摇杆值(-1,1)
        trigger_r=axe_cmds[5]   # 右扳机(-1,0)

        # 通过倍率 & 摇杆值计算目标位置增量
        self.target_pos.x+=self.__ratios.ly_ratio*ly
        self.target_pos.y+=self.__ratios.lx_ratio*lx
        self.target_pos.z+=self.__ratios.ry_ratio*ry

        # 存储cmd作为下一轮参考
        self.__last_axe_cmds=axe_cmds
        self.__last_buttom_cmds=buttom_cmds


def main(args=None):
    rclpy.init(args=args)

    # 设置初始摇杆倍率
    axes_ratios=JoyMoveArm.AxesRatio()
    axes_ratios.lx_ratio=1
    axes_ratios.ly_ratio=1
    axes_ratios.rx_ratio=1
    axes_ratios.ry_ratio=1

    node=JoyMoveArm("joy_move_arm",2,10,axes_ratios)
    rclpy.spin(node)
    rclpy.shutdown()